//
// Created by fazhehy on 2024/7/24.
//

#include "motor.h"

void left_motor_set_duty(int16_t duty)
{
    if (duty > 0){
        if (duty >= MAX_DUTY)
            duty = MAX_DUTY-1;

        DL_TimerG_setCaptureCompareValue(TIMG6, 0, DL_TIMER_CC_0_INDEX);
        DL_TimerG_setCaptureCompareValue(TIMG6, duty, DL_TIMER_CC_1_INDEX);
    }
    else{
        duty = -duty;
        if (duty >= MAX_DUTY)
            duty = MAX_DUTY-1;

        DL_TimerG_setCaptureCompareValue(TIMG6, duty, DL_TIMER_CC_0_INDEX);
        DL_TimerG_setCaptureCompareValue(TIMG6, 0, DL_TIMER_CC_1_INDEX);
    }
}

void right_motor_set_duty(int16_t duty)
{
    if (duty > 0){
        if (duty >= MAX_DUTY)
            duty = MAX_DUTY-1;

        DL_TimerG_setCaptureCompareValue(TIMG7, 0, DL_TIMER_CC_0_INDEX);
        DL_TimerG_setCaptureCompareValue(TIMG7, duty, DL_TIMER_CC_1_INDEX);
    }
    else{
        duty = -duty;
        if (duty >= MAX_DUTY)
            duty = MAX_DUTY-1;

        DL_TimerG_setCaptureCompareValue(TIMG7, duty, DL_TIMER_CC_0_INDEX);
        DL_TimerG_setCaptureCompareValue(TIMG7, 0, DL_TIMER_CC_1_INDEX);
    }
}

void motor_init()
{
    left_motor_set_duty(0);
    right_motor_set_duty(0);
}

void motor_stop()
{
    left_motor_set_duty(0);
    right_motor_set_duty(0);
}

void motor_set_duty(int16_t left, int16_t right)
{
    left_motor_set_duty(left);
    right_motor_set_duty(right);
}
